# launch/system.launch.py
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    TimerAction,
)
from launch.substitutions import (
    LaunchConfiguration,
    PathJoinSubstitution,
)
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    ugv_ids = ['ugv1', 'ugv2']
    launch_dir = PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch'])

    return LaunchDescription([


        # 启动 Gazebo 世界（空世界或含地形）
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'gzpx.launch.py']),
            launch_arguments={'world_sdf': 'ugv_world.sdf','bridge_config':'two_ugv_bridge.yaml'}.items()
        ),

        # Spawn UGV1
        IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch', 'gzugv.launch.py']),
            launch_arguments={
                'ugv_id': 'ugv1',
                'ugv_pose': '2.0,2.0,0.5,0,0,0'
            }.items()
        ),

        # Spawn UGV2（延迟一点避免冲突）



        IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch', 'gzugv.launch.py']),
            launch_arguments={
                'ugv_id': 'ugv2',
                'ugv_pose': '5.0,2.0,0.5,0,0,1.57'
            }.items()
        ),
  
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'ugv_allstart.launch.py']),
            launch_arguments={
                'ugv_ids': str(ugv_ids)  # ROS 2 支持 list 作为参数值（YAML 格式）
            }.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'addi_ugv12_bridge.launch.py'])
        ),
    ])